#include "piecewise_func.h"

namespace Nesting
{
	/////////////////////////////////////////////////////////////////////////////////////////
	//									Quad_piecewise_func								   //
	////////////////////////////////////////////////////////////////////////////////////////
	void Quad_piecewise_func::begin_insert()
	{
		open = true;
	}

	void Quad_piecewise_func::insert_piece(double start_point, double a, double b, double c)
	{
		if (breakpoints.empty() || breakpoints.back() != start_point)
		{
			breakpoints.push_back(start_point);
			coefficients.push_back(Coeff(a, b, c));
		}
		else // if the newly added piece start at the same with the last piece, they should be merged
		{
			Coeff overlay(a, b, c);
			coefficients.back() = overlay;
		}
	}

	void Quad_piecewise_func::insert_piece(double start_point, const Coeff& coeff)
	{
		insert_piece(start_point, coeff.a, coeff.b, coeff.c);
	}
// 	void Quad_piecewise_func::end_insert(double end_breakpoint)
// 	{
// 		assert(open);
// 		breakpoints.push_back(end_breakpoint);
// 		open = false;
// 	}
	void Quad_piecewise_func::end_insert()
	{
		assert(open);
		open = false;
	}
	bool Quad_piecewise_func::constant_zero() const
	{
		if (nb_pieces() == 0)
			return true;
		for (unsigned int i = 0; i < nb_pieces(); i++)
			if (coefficients[i].a != 0.0 || coefficients[i].b != 0.0 || coefficients[i].c != 0.0)
				return false;
		return true;
	}
	double Quad_piecewise_func::value_at(double x) const
	{
		if (nb_pieces() == 0 || x < breakpoints.front() || x >= breakpoints.back())
			return 0.0;
		unsigned int i;
		for ( i = 0; i < nb_pieces(); i++)
			if (breakpoints[i] > x)
				break;
		return coefficients[i-1].value_at(x);
	}
	void Quad_piecewise_func::pop_back()
	{
		if (nb_pieces() == 0)
			return;
		breakpoints.pop_back();
		coefficients.pop_back();
	}
	double Quad_piecewise_func::piece_min(unsigned int idx, double& argmin) const
	{
		if (idx == nb_pieces() - 1)
		{
			argmin = breakpoints[idx];
			return 0.0;
		}
		double min_val = std::numeric_limits<double>::max();
		double a = coefficients[idx].a, b = coefficients[idx].b, c = coefficients[idx].c;
		if (a != 0.0)
		{
			double mx = -b/2*a;
			if (mx >= breakpoints[idx] && mx <= breakpoints[idx+1])
			{
				double my = a*mx*mx + b*mx + c;
				if (min_val > my )
				{
					min_val = my;
					argmin = mx;
				}
			}
			double endy = a*breakpoints[idx]*breakpoints[idx] + b*breakpoints[idx] + c;
			if (min_val > endy)
			{
				min_val = endy;
				argmin = breakpoints[idx];
			}
 			endy = a*breakpoints[idx+1]*breakpoints[idx+1] + b*breakpoints[idx+1] + c;
 			if (min_val > endy)
 			{
 				min_val = endy;
 				argmin = breakpoints[idx+1];
 			}
		}
		else
		{
			double piece_min = b >= 0.0?(breakpoints[idx]*b + c):(breakpoints[idx+1]*b + c);
			if (min_val > piece_min)
			{
				min_val = piece_min;
				argmin = b >= 0.0?breakpoints[idx]:breakpoints[idx+1];
			}
		}
		return min_val;
	}

	double Quad_piecewise_func::min(double& argmin) const
	{
		if (nb_pieces() == 0)
			return 0.0;
		double min_val = std::numeric_limits<double>::max();
		for (unsigned int i = 0; i < nb_pieces(); i++)
		{
			double x, y;
			y = piece_min(i, x);
			if (min_val > y)
			{
				min_val = y;
				argmin = x;
			}
		}
		return min_val;
	}

	double Quad_piecewise_func::min(double l, double r, double &argmin) const
	{
		if (l > r) 
			throw Bad_range("Bad range in Quad_piecewise_func::min");
		if (l <= breakpoints.front())
		{
			argmin = breakpoints.front();
			return 0.0;
		}
		if (r >= breakpoints.back())
		{
			argmin = breakpoints.back();
			return 0.0;
		}
		unsigned int sp = 0, ep = breakpoints.size() - 1;
		while (breakpoints[sp] <= l) sp++;
		while (breakpoints[ep] >= r) ep--;
		double min_val = std::numeric_limits<double>::max();
		if (coefficients[sp-1].a != 0.0)
		{
			double valley_x = -coefficients[sp-1].b/(2*coefficients[sp-1].a);
			if (valley_x >= l && valley_x < breakpoints[sp])
			{
				double valley_y = coefficients[sp-1].value_at(valley_x);
				if (min_val > valley_y)
				{
					min_val = valley_y;
					argmin = valley_x;
				}
			}
			double end_y = coefficients[sp-1].value_at(breakpoints[sp]);
			if (min_val > end_y && breakpoints[sp] <= r)
			{
				min_val = end_y;
				argmin = breakpoints[sp];
			}
			double l_y = coefficients[sp-1].value_at(l);
			if (min_val > l_y)
			{
				min_val = l_y;
				argmin = l;
			}
		}
		else if (coefficients[sp-1].b >= 0)
		{
			double end_y = coefficients[sp-1].value_at(l);
			if (min_val > end_y)
			{
				argmin = l;
				min_val = end_y;
			}
		}
		else 
		{
			double end_y = coefficients[sp-1].value_at(breakpoints[sp]);
			if( min_val > end_y && breakpoints[sp] <= r)
			{
				argmin = breakpoints[sp];
				min_val = end_y;
			}
		}
		for (unsigned int i = sp; i < ep; i++)
		{
			double x, y;
			y = piece_min(i, x);
			if (min_val > y)
			{
				min_val = y;
				argmin = x;
			}
		}
		double db_valley_x;
		if (coefficients[ep].a != 0.0)
		{
			double valley_x = -coefficients[ep].b/(2*coefficients[ep].a);
			db_valley_x = valley_x;
			if (valley_x <= r && valley_x > breakpoints[ep])
			{
				double valley_y = coefficients[ep].value_at(valley_x);
				if (min_val > valley_y)
				{
					min_val = valley_y;
					argmin = valley_x;
				}
			}
			double end_y = coefficients[ep].value_at(breakpoints[ep]);
			if (min_val > end_y && breakpoints[ep] >= l)
			{
				min_val = end_y;
				argmin = breakpoints[ep];
			}
			double r_y = coefficients[ep].value_at(r);
			if (min_val > r_y)
			{
				min_val = r_y;
				argmin = r;
			}
		}
		else if (coefficients[ep].b >= 0.0)
		{
			double end_y = coefficients[ep].value_at(breakpoints[ep]);
			if (min_val > end_y && breakpoints[ep] >= l)
			{
				min_val = end_y;
				argmin = breakpoints[ep];
			}
		}
		else
		{
			double end_y = coefficients[ep].value_at(r);
			if (min_val > end_y)
			{
				min_val = end_y;
				argmin = r;
			}
		}
		assert(argmin >= l && argmin <= r);
		return min_val;
	}
	void add(const Quad_piecewise_func& f0, const Quad_piecewise_func& f1, Quad_piecewise_func& sum)
	{
		typedef Quad_piecewise_func::Coeff Coeff;

		if (f0.nb_pieces() == 0)
		{
			sum = f1;
			return;
		}
		else if (f1.nb_pieces() == 0)
		{
			sum = f0;
			return;
		}

		sum.begin_insert();

		unsigned int ptr0 = 0, ptr1 = 0;
		double start_breakpoint;

		// the pointer ptr0 and ptr1 are always one step further than coefficients, coeff_f0 and coeff_f1
		Coeff coeff_f0(0.0, 0.0, 0.0), coeff_f1(0.0, 0.0, 0.0); // f0.coefficient[-1], f1.coefficient[-1]

		while (ptr0 < f0.nb_pieces() && ptr1 < f1.nb_pieces())
		{
			if (f0.breakpoints.at(ptr0) < f1.breakpoints.at(ptr1))
			{
				coeff_f0 = f0.coefficients.at(ptr0);
				start_breakpoint = f0.breakpoints.at(ptr0);
				ptr0++;
			}
			else if (f0.breakpoints.at(ptr0) > f1.breakpoints.at(ptr1))
			{
				coeff_f1 = f1.coefficients.at(ptr1);
				start_breakpoint = f1.breakpoints.at(ptr1);
				ptr1++;
			}
 			else
 			{
				start_breakpoint = f0.breakpoints.at(ptr0);
 				coeff_f0 = f0.coefficients.at(ptr0);
 				ptr0++;
				coeff_f1 = f1.coefficients.at(ptr1);
				ptr1++;				
 			}
			sum.insert_piece(
					start_breakpoint,
					coeff_f0.a + coeff_f1.a, 
					coeff_f0.b + coeff_f1.b,
					coeff_f0.c + coeff_f1.c
					);
		}
		while (ptr0 < f0.nb_pieces())
		{
			start_breakpoint = f0.breakpoints.at(ptr0);
			coeff_f0 = f0.coefficients.at(ptr0);
			ptr0++;
			sum.insert_piece(
				start_breakpoint,
				coeff_f0.a + coeff_f1.a, 
				coeff_f0.b + coeff_f1.b,
				coeff_f0.c + coeff_f1.c
				);
		}
		while (ptr1 < f1.nb_pieces())
		{
			start_breakpoint = f1.breakpoints.at(ptr1);
			coeff_f1 = f1.coefficients.at(ptr1);
			ptr1++;
			sum.insert_piece(
				start_breakpoint,
				coeff_f0.a + coeff_f1.a, 
				coeff_f0.b + coeff_f1.b,
				coeff_f0.c + coeff_f1.c
				);
		}
		sum.end_insert();

// 		of->breakpoints.pop_back();
// 		of->coefficients.pop_back();		
	}

	void Quad_piecewise_func::print()
	{
		for (unsigned int i = 0; i < nb_pieces(); i++)
		{
			std::cout<<'['<<breakpoints[i]<<','<<breakpoints[i+1]<<"]: ";
			std::cout<<"a = "<<coefficients[i].a<<", b = "<<coefficients[i].b<<", c = "<<coefficients[i].c;
			std::cout<<std::endl;
		}
		double min_x, min_y;
		min_y = min(min_x);
		std::cout<<"Min = "<<min_y<<" at x = "<<min_x<<std::endl;
	}

}